package org.scalpel.HHObjects;

import ioio.lib.api.exception.ConnectionLostException;

import org.scalpel.common.HHObject;
import org.scalpel.common.KeyPoint;
//import org.hypheli.testApps.R;

import org.opencv.android.Utils;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;

import android.content.Context;
import android.graphics.Bitmap;
import android.graphics.BitmapFactory;
import android.graphics.BitmapFactory.Options;
import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;
import android.graphics.Rect;


/**
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * Plot the obstacle probability map. 
 * It give a callback to draw robot position because we consider
 * that there is no obstacle where the robot currently is.
 **/
public class MapManager extends HHObject {

	// Used to read map from resources
	private Context fContext = null;
	
	// Resources Map Id 
	private int fMapId = -1;
	
	// Conversion factor between meters and pixels
	private Double fMeterToPix;
	
	// Size of the image
	private int fWidth;
	private int fHeight;
	
	// Probability Bitmap  
	private Bitmap fProbaImage1 = null;
	private Bitmap fProbaImage = null;
	private Mat fProbaIpl = null;
	private Mat fProbaIplBGRA = null;

	// Canvas and paint used to draw proba on the image
	private Canvas fCanvas = null;
	private Paint fPaint = null;
	
	// Optimization
	private Object[] fParamDrawObstacle = null;
	
	private int fZoomFactor = 1;
	private Rect fSrcRect;
	private Rect fDestRect;
	
	public MapManager(String fName, int mapid, Double pixByMeter, int zoomFactor) {
		super(fName);
		fWidth = 600;
		fHeight = 360;
		fMapId = mapid;
		fMeterToPix = pixByMeter;
		fZoomFactor = zoomFactor;

	}

	public static Point rotationPoint( double ptx, double pty, KeyPoint centre, double angleRad ){
		/* Rotation du point pt autour du point centre
		*/

		Point res = new Point();
		
		res.x = Math.cos(angleRad)*(ptx - centre.x) + Math.sin(angleRad)*(pty - centre.y) + centre.x;
		res.y = -Math.sin(angleRad)*(ptx - centre.x) + Math.cos(angleRad)*(pty - centre.y) + centre.y;
		
		return res;
	}
	
	public static Point rotationPoint( double ptx, double pty, KeyPoint centre, double angleRad, double meterToPix ){
		/* Rotation du point pt autour du point centre
		*/

		Point res = new Point();

		res.x = meterToPix * (Math.cos(angleRad)*(ptx - centre.x) + Math.sin(angleRad)*(pty - centre.y) + centre.x);
		res.y = meterToPix * (-Math.sin(angleRad)*(ptx - centre.x) + Math.cos(angleRad)*(pty - centre.y) + centre.y);
		
		return res;
	}
	

	synchronized public void setLineProbability( KeyPoint pStart, KeyPoint pEnd, double lineProba, double endPointProba ){
		if( fCanvas == null ) return;
		
		int g = (int)(lineProba * 0xffffff);
		
		int xs = (int)(pStart.x * fMeterToPix)/fZoomFactor;
		int ys = (int)(pStart.y * fMeterToPix)/fZoomFactor;
		int xe = (int)(pEnd.x * fMeterToPix)/fZoomFactor;
		int ye = (int)(pEnd.y * fMeterToPix)/fZoomFactor;

		fPaint.setARGB(255, g, g, g);
		fCanvas.drawLine(xs, ys, xe, ye, fPaint);
	
	}
	
	synchronized public void setRectProbability( KeyPoint p, double width, double height, double probability ){
		if( fCanvas == null ) return;
		
		int left = (int)(p.x * fMeterToPix)/fZoomFactor;
		int w = (int)(width * fMeterToPix)/fZoomFactor;
		int h = (int)(height * fMeterToPix)/fZoomFactor;
		int top = fHeight - (int)((p.y + height)* fMeterToPix)/fZoomFactor;

		int g = (int)(1.-probability) * 0xffffff;
		fPaint.setARGB(255, g, g, g);
		fCanvas.drawRect(left, top, left+w, top+h, fPaint);
		return;
	}
	
  public void updateIOIO() throws ConnectionLostException 
  {
  	if( fCanvas == null) return;

		fPaint.setARGB(255, 0, 0, 0);
  	fPaint.setAntiAlias(true);
  	fPaint.setDither(false);
  	fPaint.setFilterBitmap(false);

		fCanvas.save();
		
		setChanged();
		if( fParamDrawObstacle == null )
			fParamDrawObstacle = new Object[]{"drawProba", new Object[]{fCanvas, fPaint, fProbaIpl, fZoomFactor}};
		notifyObservers(fParamDrawObstacle);
		
		fCanvas.restore();
  };

  synchronized public void onInitView(Context context) {

		fContext = context;
		Options opt = new Options(); 
		opt.inPreferredConfig = Bitmap.Config.ARGB_8888;//ALPHA_8;
		opt.inDither = false;
		
		/*
		int r = R.drawable.gamefield;
		int r = R.drawable.mapobstacle2;
  	*/
		fProbaImage = BitmapFactory.decodeResource( fContext.getResources(), fMapId, opt);
  	fProbaImage1 = Bitmap.createBitmap(fProbaImage.getWidth(), fProbaImage.getHeight(), opt.inPreferredConfig);
  	
  	fProbaIpl = new Mat(fProbaImage.getWidth(), fProbaImage.getHeight(), CvType.CV_8U);
  	fProbaIplBGRA = new Mat(fProbaImage.getWidth(), fProbaImage.getHeight(), CvType.CV_8UC(4));
  		
  	fProbaIplBGRA = Utils.bitmapToMat(fProbaImage);
  	Imgproc.cvtColor(fProbaIplBGRA, fProbaIpl, Imgproc.COLOR_BGR2GRAY);
  	
 		fWidth = fProbaImage1.getWidth();
  	fHeight = fProbaImage1.getHeight();
  	fCanvas = new Canvas();
  	fPaint = new Paint();
  	
		fSrcRect = new Rect();
  	fDestRect = new Rect();
  	fSrcRect.set(0, 0, fWidth, fHeight);
  	fDestRect.set(0, 0, fWidth*fZoomFactor, fHeight*fZoomFactor);


  };

  public void onDraw(Canvas canvas, Paint robotPaint){
  	canvas.save();
  	robotPaint.setColor(Color.WHITE); 

  	Imgproc.cvtColor(fProbaIpl, fProbaIplBGRA, Imgproc.COLOR_GRAY2BGRA,4);
  	Utils.matToBitmap(fProbaIplBGRA, fProbaImage1);

  	robotPaint.setAntiAlias(true);
  	robotPaint.setDither(false);
  	robotPaint.setFilterBitmap(false);
  	canvas.drawBitmap(fProbaImage1, fSrcRect, fDestRect, robotPaint);
  	
  	robotPaint.setColor(Color.DKGRAY);
  	canvas.drawLine(fDestRect.right, fDestRect.top, fDestRect.right, fDestRect.bottom, robotPaint);
/*
  	robotPaint.setARGB(255, 255, 0, 0);
  	robotPaint.setStrokeWidth(2);
  	robotPaint.setStyle(Style.STROKE);

  	//
  	
  	canvas.drawRect(fDestRect, robotPaint);
*/
  	canvas.restore();
  }

	@Override
	public void finish() {
		// TODO Auto-generated method stub
		
	};
}
